#!/usr/bin/python
import numpy as np
import os
import matplotlib.pyplot as plt
DNA_SIZE = 10
POP_SIZE = 20
CROSSOVER_RATE = 0.3
MUTATION_RATE = 0.05
X_BOUND = [0, 100]
Y_BOUND = [0, 100]
Z_BOUND = [0, 100]
def translateDNA(pop):
    o_pop = pop[:, :1]#old_navfn_behavior
    u1_pop = pop[:, 1:2]#use_quadratic
    u2_pop = pop[:, 2:3]#use_dijkstra
    u3_pop = pop[:, 3:4]#use_grid_path
    a_pop = pop[:, 4:5]#allow_unknown
    x_pop = pop[:, 5:15]#planner_window_x
    y_pop = pop[:, 15:25]#planner_window_y
    z_pop = pop[:, 25:35]# default_tolerance
    p_pop = pop[:, 35:]#publish_scale,
    o = o_pop.dot(2 ** np.arange(1)[::-1])
    u1 = u1_pop.dot(2 ** np.arange(1)[::-1])
    u2 = u2_pop.dot(2 ** np.arange(1)[::-1])
    u3 = u3_pop.dot(2 ** np.arange(1)[::-1])
    a = a_pop.dot(2 ** np.arange(1)[::-1])
    x = x_pop.dot(2 ** np.arange(DNA_SIZE)[::-1]) / float(2 ** DNA_SIZE - 1) * (X_BOUND[1] - X_BOUND[0]) + X_BOUND[0]
    y = y_pop.dot(2 ** np.arange(DNA_SIZE)[::-1]) / float(2 ** DNA_SIZE - 1) * (Y_BOUND[1] - Y_BOUND[0]) + Y_BOUND[0]
    z = z_pop.dot(2 ** np.arange(DNA_SIZE)[::-1]) / float(2 ** DNA_SIZE - 1) * (Z_BOUND[1] - Z_BOUND[0]) + Z_BOUND[0]
    p = p_pop.dot(2 ** np.arange(6)[::-1])
    return o, u1, u2, u3,a, x, y, z, p

def analysisDNA(pop):
    pop1 = pop[:, :5]
    pop2 = pop[:, 5:15]
    pop3 = pop[:, 15:25]
    pop4 = pop[:, 25:35]
    pop5 = pop[:, 35:]
    return pop1, pop2, pop3, pop4, pop5

def mergeDNA(pop1, pop2, pop3, pop4, pop5):
    newpop = np.concatenate((pop1, pop2, pop3, pop4, pop5), axis=1)    
    return newpop

def crossover_and_mutation(pop, SIZE, CROSSOVER_RATE=0.3):
    new_pop = []
    for father in pop:
        child = father
        if np.random.rand() < CROSSOVER_RATE:
            mother = pop[np.random.randint(POP_SIZE/2)]
            cross_points = np.random.randint(low=0, high=SIZE) 
            child[cross_points:] = mother[cross_points:]
        mutation(child, SIZE)
        new_pop.append(child)
    a_list = list(pop)
    b_list = list(new_pop)
    a_list.extend(b_list)
    return np.array(a_list)
    #return new_pop

def mutation(child, SIZE, MUTATION_RATE=0.05):
    if np.random.rand() < MUTATION_RATE:  
        mutate_point = np.random.randint(SIZE)
        child[mutate_point] = child[mutate_point] ^ 1  

def read_result(path):
    with open(path, "r") as file:
        lines = file.readlines()
    length = []
    steps = []
    time = []
    for line in lines:
        sp = line.split('\t')
        # print(sp)
        if len(sp) == 3:
            length.append(float(sp[0].strip()))
            steps.append(float(sp[1].strip()))
            time.append(int(sp[2].strip()))
        else:
            length.append(float(1000))
            steps.append(float(0))
            time.append(int(1000))

    return length, steps, time

def fitness(length, steps, time):
    list_norm(length)
    list_norm(steps)
    list_norm(time)
    # print(length,steps,time)
    fit_l = []
    for i in range(len(length)):
        l = length[i]
        s = steps[i]
        t = time[i]
        #fit = 0.4 * l + 0.2 * s + 0.4 * t
	fit=t
        # print('fitness'+str(fit))
        fit_l.append(fit)
    list_norm(fit_l)
    return fit_l

def list_norm(l):
    max_l = max(l)
    min_l = min(l)
    range_l = max_l - min_l
    for i in range(len(l)):
	if range_l == 0:
	    l[i] = 1
	else:
            l[i] = (l[i] - min_l) / range_l

def choose(pop, fitness):
    for i in range(POP_SIZE):
        for j in range(POP_SIZE):
            if fitness[i] > fitness[j]:
		fitness[i], fitness[j] = fitness[j], fitness[i]
		pop[i], pop[j] = pop[j], pop[i]
    return pop[:POP_SIZE/2]

def write_yaml(pop,curpath):
    o, u1, u2, u3, a, x, y, z, p = translateDNA(pop)
    for i in range(POP_SIZE):
        yamlpath = os.path.join(curpath, str(i) + '.yaml')
        with open(yamlpath, "w") as f:
            f.write('GlobalPlanner:\n')
            f.write(' old_navfn_behavior'+': '+str(bool(o[i]))+'\n')
            f.write(' use_quadratic'+': '+str(bool(u1[i]))+'\n')
            f.write(' use_dijkstra'+': '+str(bool(u2[i]))+'\n')
            f.write(' use_grid_path'+': '+str(bool(u3[i]))+'\n')
            f.write(' allow_unknown'+': '+str(bool(a[i]))+'\n')
            f.write(' planner_window_x'+': '+str(x[i])+'\n')
            f.write(' planner_window_y'+': '+str(y[i])+'\n')
            f.write(' default_tolerance'+': '+str(z[i])+'\n')
            f.write(' publish_scale'+': '+str(p[i]))

def write_pop(pop,path):
    write_path = os.path.join(path,'pop.txt')
    with open(write_path, "w") as f:
        for i in range(POP_SIZE): 
            p=pop[i]
            for j in range(len(p)-1):
                f.write(str(p[j])+' ')
            f.write(str(p[len(p)-1])+'\n')

def read_pop(path):
    pop=[]
    write_path = os.path.join(path,'pop.txt')
    with open(write_path, "r") as f:
        lines=f.readlines()
	for line in lines:
            p=[]
            sp=line.split(' ')
            for s in sp:
                p.append(int(s))
            pop.append(p)
    return pop

if __name__ == "__main__":
    pop_path = '/home/nero/gen_pop'
    pop = read_pop(pop_path)  
    pop = np.array(pop)
    #print("reading POP %s" %pop)
    res_path = '/home/nero/res.txt'
    length, steps, time = read_result(res_path)
    #print(length, steps, time)
    fit = fitness(length, steps, time)
    #print(fit)
    pop_parents = choose(pop, fit)
    pop1, pop2, pop3, pop4, pop5 = analysisDNA(pop_parents)
    #print(pop1, pop2, pop3, pop4, pop5)
    pop1 = crossover_and_mutation(pop1, 5, CROSSOVER_RATE)
    pop2 = crossover_and_mutation(pop2, 10, CROSSOVER_RATE)
    pop3 = crossover_and_mutation(pop3, 10, CROSSOVER_RATE)
    pop4 = crossover_and_mutation(pop4, 10, CROSSOVER_RATE)
    pop5 = crossover_and_mutation(pop5, 6, CROSSOVER_RATE)
    pop = mergeDNA(pop1, pop2, pop3, pop4, pop5)
    gen_path = '/home/nero/yaml_gen2'
    write_yaml(pop,gen_path)
    write_pop(pop,pop_path)

   
